import os
import yaml
from ament_index_python.packages import get_package_share_directory
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import ComposableNodeContainer, Node
from launch.actions import TimerAction, Shutdown
from launch import LaunchDescription
from launch.substitutions import Command

def generate_launch_description():
    # 加载参数文件路径
    node_params = os.path.join(get_package_share_directory('rm_startup'), 'config', 'rm_node_parameters.yaml')

    launch_params_path = os.path.join(get_package_share_directory('rm_startup'), 'config', 'rm_launch_parameters.yaml')
    launch_params = yaml.safe_load(open(launch_params_path))

    # 定义一个函数用于创建相机节点
    def get_camera_node(package, plugin):
        return ComposableNode(
            package=package,
            plugin=plugin,
            name='camera_node',
            parameters=[node_params],
            extra_arguments=[{'use_intra_process_comms': True}],
        )
    
    # 创建相机节点
    hik_camera_node = get_camera_node('hik_camera', 'hik_camera::HikCameraNode')
    mv_camera_node = get_camera_node('mindvision_camera', 'mindvision_camera::MVCameraNode')

    # 根据启动参数选择相机节点
    camera_node = None  # 默认为None，避免未定义问题
    if launch_params.get('camera') == 'hik_camera':
        camera_node = hik_camera_node
    elif launch_params.get('camera') == 'mv_camera':
        camera_node = mv_camera_node

    # 创建飞镖校准节点
    darts_adjust_node = Node(
        package='darts_adjust_node',
        executable='darts_adjust_node',
        name='darts_adjust_node',
        parameters=[node_params],
        output='both',
    )

    # 创建串口驱动节点
    serial_driver_node = Node(
        package='rm_serial_driver',
        executable='rm_serial_driver_node',
        name='serial_driver',
        parameters=[node_params],
        output='both',
    )
    
    # 创建视频录制节点
    video_recorder_node = Node(
        package='video_recorder',
        executable='video_recorder_main',
        name='video_recorder_node',
        parameters=[node_params],
        output='both',
    )

    # 延迟启动串口驱动节点
    serial_driver_node_delay = TimerAction(
        period=3.0,
        actions=[serial_driver_node],
    )

    # 延迟启动飞镖校准节点
    darts_adjust_node_delay = TimerAction(
        period=3.0,
        actions=[darts_adjust_node],
    )
    
    # 延迟启动视频录制节点（确保相机节点已启动）
    video_recorder_node_delay = TimerAction(
        period=5.0,
        actions=[video_recorder_node],
    )

    # 创建组件容器
    container = None
    if camera_node is not None:
        container = ComposableNodeContainer(
            name='camera_container',
            namespace='',
            package='rclcpp_components',
            executable='component_container',
            composable_node_descriptions=[camera_node],
            output='screen',
        )

    # 返回启动描述，包含所有节点和容器
    launch_entities = [serial_driver_node_delay, darts_adjust_node_delay, video_recorder_node_delay]
    if container is not None:
        launch_entities.append(container)
        
    return LaunchDescription(launch_entities)